#ifndef __USER_PROG_
#define __USER_PROG_


#include "stdint.h"
#include "stdbool.h"
#include "Ano_ProgramCtrl_User.h"

//单位:10ms
#define MAX_COUNTDOWN 100 * 8
#define TARGETALTITUDECM 50
#define MAX_HOVER_ERR 10
#define MAX_FORMCHANGE_TIME 3
#define MAX_HOVER_TIME 300
#define MAX_TIMEOUT1 10000
#define MAX_TIMEOUT2 1500
#define MAX_ALT_ERR 10

#include "Ano_ProgramCtrl_User.h"



/*

*/

typedef enum
{
    CmdNone = 0,
    CmdTakeOff,
    CmdLand,
    CmdUp,
    CmdDown,
    CmdForward,
    CmdBack,
    CmdLeft,
    CmdRight,
    CmdCCW,
    CmdCW,
    CmdEmergencyShutDown,
    CmdSpeeedControl,

    NumofCmd,
} FMUCmd_t;



void Position_Init();
typedef enum
{
	
	//通用数据
    ActionWaitting = 0,
    ActionCountdown,
    ActionTakeOff,
    ActionHoverStartPoint,
	
    Action_A0,
    Action_A1,
    Action_A2,
    Action_A3,
    Action_A4,
    Action_A5,
    Action_A6,
    Action_A7,
    Action_A8,
    Action_A9,
	  Action_A10,
	  Action_A11,
	  Action_A12,
	
	  Action_Input_mode_position_1,
		Action_Input_mode_position_2,
		Action_Input_mode_2_position_1,
		Action_Input_mode_2_position_2,

	Action_CV_Point,
	Action_Z_80CM,//下降到80cm
	Action_Z_150CM,//上升到150cm
	
		Action_Land,
    NumofActionList,
} FSMList_t;


typedef struct
{
    int16_t x;
    int16_t y;
	  int16_t z;
    uint8_t code;
} Point_t;

//数据结构声明
// #pragma pack(1)
typedef struct
{
    Point_t Pos1;
    Point_t Pos2;
    Point_t Pos3;
    Point_t Pos4;
    Point_t Pos5;
    Point_t Pos6;
	uint8_t Name;
} Position;



typedef struct
{
    int16_t Start;
    uint8_t Start2;
    Point_t CentPoint;
    int16_t End;
} OpenMVFrame_t;

#pragma pack()

typedef struct
{
    uint16_t SonarF;
    uint16_t SonarB;
    uint16_t SonarL;
    uint16_t SonarR;
} SonarManager_t;

typedef enum
{
    searching = 0,
    done,
    fail,
    frame_err,

    list_searching,
} enSearch_state_t;




typedef struct
{
    int CountDownNumMs;
    FSMList_t ActionList;
	  FSMList_t Action_Last;
    FSMList_t Action_IRQ;
    float distance;
    bool ActionComplete;
	
} FollowManager_t;



//typedef struct
//{
//    //动作序列
//    ActList_t ActList;

//} ShelveManager_t;

extern FollowManager_t FollowManager;
extern SonarManager_t SonarManager;

void Follow_Init(void);
void UpdateStatus(float dT_s,s16 *CH_N);
void ActionIRQ_Request(FSMList_t Action_Req);

void Test_ProgramCtrl(float dT_s,s16 *CH_N);

///////////////
void Input_mode_position_Get_Byte(u8 data);
void Input_mode_send(u8 mode_select,u8 Data_bit_selection,u8 mode_state);
void Action_mode_send(u8 mode_select,u8 mode_state);
extern uint8_t Input_mode_2_Learning_color_blocks;//学习的色块
void sand_IF_Learning_color_blocks();
///////////////




#endif
